#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/passthrough.h>

#define Z_TOLERANCE 0.02
#define Z_TOLERANCE_DOWN 0.28  //0.24
#define Z_TOLERANCE_UP 0.2  //0.1
#define Z_OFFSET 0.3
int
 main (int argc, char** argv)
{
	if (argc < 3) {
		printf("%s input_pcl output_pcl\n", argv[0]);
		return 1;
	}
	ros::init(argc, argv, "planaer_tester");
	ros::NodeHandle nh;
	ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>(
			"/rgbdslam/aggregate_clouds", 1);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

  if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (argv[1], *cloud) == -1) //* load the file
  {
    ROS_ERROR ("Couldn't read file SURF.pcd \n");
    return (-1);
  }
  std::cout << "Loaded "
            << cloud->width * cloud->height
            << " data points from test_pcd.pcd with the following fields: "
            << std::endl;

	pcl::PointCloud<pcl::PointXYZRGB> flat_cloud;
	pcl::PassThrough<pcl::PointXYZRGB> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName ("z");
  pass.setFilterLimits (Z_OFFSET - Z_TOLERANCE_DOWN,
			Z_OFFSET + Z_TOLERANCE_UP);

	pass.filter(flat_cloud);
	//for (size_t i = 0; i < flat_cloud.points.size(); ++i) {
  //  flat_cloud.points[i].z = 0;
	//}
  
	sensor_msgs::PointCloud2 cloud_msg;
	pcl::toROSMsg(*cloud, cloud_msg);
	ROS_INFO("Publish msg %d %d", cloud_msg.point_step,
			cloud_msg.row_step);
  pcl_pub.publish(cloud_msg);
	ROS_INFO("Published msg");

	pcl::io::savePCDFileASCII(argv[2], flat_cloud);
  return (0);
}
